
#include "BridgeIOConfig.hpp"
#include "cybertron/core/UtilString.hpp"
#include "cybertron/core/JsonReader.hpp"

//#include "Node/Coordinator.pb.h"
#include "Node/Agent.pb.h"

CYBERTRON_BEGIN

std::string BridgeConfig::mSensorCameraName = "camera";
std::string BridgeConfig::mSensorObjectBasedCameraName = "objectBasedCamera";
std::string BridgeConfig::mSensorLidarName = "lidar";
std::string BridgeConfig::mSensorObjectBasedLidarName = "objectBasedLidar";
std::string BridgeConfig::mSensorAllUltrasonicRadarName = "ultrasonicRadars";
std::string BridgeConfig::mSensorUltrasonicRadarName = "ultrasonic";
std::string BridgeConfig::mSensorRadarName = "radar";
std::string BridgeConfig::mSensorObjectBasedRadarName = "objectBasedRadar";
std::string BridgeConfig::mSensorPerfectPerception = "perfectPerception";
std::string BridgeConfig::mSensorIMU_GPSName = "imu";
std::string BridgeConfig::mSensorV2XName = "v2x";
std::string BridgeConfig::mSensorFusionName = "sensorFusion";

BridgeConfig::BridgeConfig()
{
	zeroMembers();
}

BridgeConfig::~BridgeConfig()
{
	clear();
}

void BridgeConfig::clear()
{
	zeroMembers();
}

bool BridgeConfig::loadConfig(Agent::WorkNodeRegisterRet* args, CybertronBridgeIO*p)
{
 	clear();
 	cybertron::json workRoot;
 	cybertron::json worldRoot;
	std::string worldConfigJson = args->startcommand().worldconfigjson();
	if (!JsonReader::loadString(worldRoot, worldConfigJson)){
		return false;
	}
	auto workRootJson = JsonReader::getChild(worldRoot, "configScene");
	mTaskId = args->startcommand().taskid();
	mOD = cybertron::JsonReader::getString(workRootJson, "openDrive");
	mODUrl = cybertron::JsonReader::getString(workRootJson, "openDriveUrl");
	mODMd5 = cybertron::JsonReader::getString(workRootJson, "openDriveMd5");

	memcpy(mCaseInfo.caseName, args->startcommand().config().casename().c_str(), args->startcommand().config().casename().size());
	memcpy(mCaseInfo.caseId, args->startcommand().config().caseid().c_str(), args->startcommand().config().caseid().size());
	memcpy(mCaseInfo.taskId, mTaskId.c_str(), mTaskId.size());

	memcpy(mHDMapInfo.openDrive, mOD.c_str(), mOD.size());
	memcpy(mHDMapInfo.openDriveUrl, mODUrl.c_str(), mODUrl.size());
	memcpy(mHDMapInfo.opendriveMd5, mODMd5.c_str(), mODMd5.size());
	if (cybertron::JsonReader::hasValue(workRootJson, "weather")) {
		auto jsonWeather = cybertron::JsonReader::getChild(workRootJson, "weather");
		if (cybertron::JsonReader::hasValue(jsonWeather, "timeOfDay")) {
			mWeatherConfig.timeOfDay = (float)cybertron::JsonReader::getDouble(jsonWeather, "timeOfDay");
			mWeatherConfig.directionalLight = (float)cybertron::JsonReader::getDouble(jsonWeather, "lightIntensity");
			float ambientLight = (float)cybertron::JsonReader::getDouble(jsonWeather, "ambientLightIntensity");
			if (ambientLight < 1e-4) ambientLight = mWeatherConfig.directionalLight;
			mWeatherConfig.ambientLight = ambientLight;
			float artificialLight = (float)cybertron::JsonReader::getDouble(jsonWeather, "artificialLightIntensity");
			if (artificialLight < 1e-4) artificialLight = mWeatherConfig.directionalLight;
			mWeatherConfig.artificialLight = artificialLight;
			mWeatherConfig.cloudDensity = (float)cybertron::JsonReader::getDouble(jsonWeather, "cloudDensity");
			mWeatherConfig.fogDensity = (float)cybertron::JsonReader::getDouble(jsonWeather, "fogDensity");
			mWeatherConfig.rainDensity = (float)cybertron::JsonReader::getDouble(jsonWeather, "rainDensity");
			mWeatherConfig.snowDensity = (float)cybertron::JsonReader::getDouble(jsonWeather, "snowDensity");
			mWeatherConfig.heightAngle = (float)cybertron::JsonReader::getDouble(jsonWeather, "heightAngle") * 90;
			if (cybertron::JsonReader::hasValue(jsonWeather, "ground")) {
				auto jsonGround = cybertron::JsonReader::getChild(jsonWeather, "ground");
				mWeatherConfig.groundHumidityLevel = (float)cybertron::JsonReader::getDouble(jsonGround, "humidityLevel");
				mWeatherConfig.groundDirtyLevel = (float)cybertron::JsonReader::getDouble(jsonGround, "dirtyLevel");
			}
		}
	}

	if (!p->getWorkJsonFromWeb(workRoot)) {
		return false;
	}
	for (size_t i = 0; i < workRoot.size(); ++i) {
		if (JsonReader::hasValue(workRoot[i], "customization")) {
			auto jsonCustomization = JsonReader::getChild(workRoot[i], "customization");
			if (JsonReader::hasValue(jsonCustomization, "model")) {
				runMode = JsonReader::getString(jsonCustomization, "model");
			}
			if (JsonReader::hasValue(jsonCustomization, "projectType")) {
				simulinkType = JsonReader::getString(jsonCustomization, "projectType");
			}
			if (JsonReader::hasValue(jsonCustomization, "entry")) {
				simulinkPath = JsonReader::getString(jsonCustomization, "entry");
			}
			if (JsonReader::hasValue(jsonCustomization, "level")) {
				controlLevel = JsonReader::getString(jsonCustomization, "level");
			}
		}
		HostVehicleConfig mHostVehicleConfig;
		int hostVehicleId = JsonReader::getInt(workRoot[i], "mainVehicleId");
		std::string strVehicleId = std::to_string(hostVehicleId);
		mHostVehicleConfig.vehicleId = strVehicleId;
		if (JsonReader::hasValue(workRoot, "hz")) {
			mHostVehicleConfig.vehicleHZ = JsonReader::getInt(workRoot[i], "hz");
		}
		mHostVehicleId = atoi(mHostVehicleConfig.vehicleId.c_str());
		mHostVehicleHz = mHostVehicleConfig.vehicleHZ;
		logOutputInfo(mTaskId.c_str(), "Vehicle ID: %d, Vehicle HZ: %d", hostVehicleId, mHostVehicleConfig.vehicleHZ);
		std::string levelStr = JsonReader::getString(workRoot[i], "level");
		mHostVehicleConfig.driverSet = (levelStr == "L2");

		NodeInstance vehicleNode;
		vehicleNode.instanceId = JsonReader::getInt(workRoot[i], "vehicleNodeInstanceId");
		vehicleNode.friendlyId = "vehicle" + std::to_string(vehicleNode.instanceId);
		vehicleNode.type = ESimOne_Node_Type_Vehicle;
		vehicleNode.valid = true;
		mHostVehicleConfig.nodes.push_back(vehicleNode);	// Note: vehicle node should be the first node

		bool skipCameraSensor = false;
		auto customizationNode = JsonReader::getChild(workRoot[i], "customization");
		if (JsonReader::hasValue(customizationNode, "skipCameraSensor")) {
			skipCameraSensor = JsonReader::getBool(customizationNode, "skipCameraSensor");
			logOutputInfo(mTaskId.c_str(), "skipCameraSensor: %d", skipCameraSensor);
		}
		if (JsonReader::hasValue(customizationNode, "startScriptPath")) {
			mHostVehicleConfig.startScriptPath = cybertron::JsonReader::getString(customizationNode, "startScriptPath");
			logOutputInfo(mTaskId.c_str(), "Start scripts is %s", mHostVehicleConfig.startScriptPath.c_str());
		}
		else {
			logOutputInfo(mTaskId.c_str(), "NO Start scripts.");
			mHostVehicleConfig.startScriptPath = "";
		}
		if (JsonReader::hasValue(customizationNode, "endScriptPath")) {
			mHostVehicleConfig.endScriptPath = cybertron::JsonReader::getString(customizationNode, "endScriptPath");
		}
		else {
			mHostVehicleConfig.endScriptPath = "";
		}

		mHostVehicleConfig.sensorConfigurations.dataSize = 0;
		if (JsonReader::hasValue(workRoot[i], "generalSensors")) {
			auto jsonSensors = JsonReader::getChild(workRoot[i], "generalSensors");
			for (size_t i = 0; i < jsonSensors.size(); ++i) {
				NodeInstance sensorNode;
				auto& jsonSensor = jsonSensors[i];
				sensorNode.valid = false;
				std::string type = JsonReader::getString(jsonSensor, "type");

				if (type == mSensorCameraName) {
					if (!skipCameraSensor) {
						sensorNode.type = ESimOne_Node_Type_Camera;
					}
					else {
						continue;
					}
				}
				else if (type == mSensorObjectBasedCameraName) {
					sensorNode.type = ESimOne_Node_Type_Camera;
				}
				else if (type == mSensorLidarName) {
					sensorNode.type = ESimOne_Node_Type_LiDAR;
				}
				else if (type == mSensorObjectBasedLidarName) {
					sensorNode.type = ESimOne_Node_Type_LiDAR;
				}
				else if (type == mSensorPerfectPerception) {
					sensorNode.type = ESimOne_Node_Type_PerfectPerception;
				}
				else if (type == mSensorAllUltrasonicRadarName) {
					sensorNode.type = ESimOne_Node_Type_AllUltrasonicRadar;
				}
				else if (type == mSensorUltrasonicRadarName) {
					sensorNode.type = ESimOne_Node_Type_UltrasonicRadar;
				}
				else if (type == mSensorRadarName || type == mSensorObjectBasedRadarName) {
					sensorNode.type = ESimOne_Node_Type_MMWRadar;
				}
				else if (type == mSensorIMU_GPSName) {
					sensorNode.type = ESimOne_Node_Type_GNSSINS;
				}
				else if (type == mSensorV2XName) {
					sensorNode.type = ESimOne_Node_Type_V2X;
				}
				else if (type == mSensorFusionName) {
					sensorNode.type = ESimOne_Node_Type_SensorFusion;
				}

				sensorNode.alias = JsonReader::getString(jsonSensor, "alias");
				sensorNode.instanceId = JsonReader::getInt(jsonSensor, "instanceId");
				sensorNode.friendlyId = JsonReader::getString(jsonSensor, "friendlyId");

				if (sensorNode.friendlyId == "v2x" || sensorNode.friendlyId.find("roadsideRsu")!= -1|| sensorNode.friendlyId == "ultrasonicRadars") {
					continue;
				}
				auto& data = JsonReader::getChild(jsonSensor, "data");
				auto& params = JsonReader::getChild(data, "params");

				sensorNode.hz = (int)JsonReader::getDouble(params, "frequency");

				mHostVehicleConfig.nodes.push_back(sensorNode);
				
				// for IMU settings
				if (type == mSensorIMU_GPSName) {
					SensorConfigUtil::loadConfig(data, mHostVehicleConfig.sensorGPSImuConfig);
					// reset imu hz to be vehicle hz at most for simulation.
					if (mHostVehicleConfig.sensorGPSImuConfig.HzImu > mHostVehicleConfig.vehicleHZ) {
						mHostVehicleConfig.sensorGPSImuConfig.HzImu = (float)mHostVehicleConfig.vehicleHZ;
					}
				}
				// backup all sensor configuration.
				SimOne_Data_SensorConfiguration& sensorConfig = mHostVehicleConfig.sensorConfigurations.data[mHostVehicleConfig.sensorConfigurations.dataSize];
				memset(sensorConfig.mainVehicleId, 0, SENSOR_IDTYPE_MAX);
				memcpy(sensorConfig.mainVehicleId, strVehicleId.c_str(), strVehicleId.size());
				sensorConfig.id = sensorNode.instanceId;

				memset(sensorConfig.sensorId, 0, SENSOR_IDTYPE_MAX);
				memcpy(sensorConfig.sensorId, sensorNode.friendlyId.c_str(), sensorNode.friendlyId.size());
				memset(sensorConfig.sensorType, 0, SENSOR_IDTYPE_MAX);
				memcpy(sensorConfig.sensorType, type.c_str(), type.size());

				sensorConfig.hz = sensorNode.hz;
				sensorConfig.x = (float)JsonReader::getDouble(data, "x");
				sensorConfig.y = (float)JsonReader::getDouble(data, "y");
				sensorConfig.z = (float)JsonReader::getDouble(data, "z");
				sensorConfig.roll = (float)JsonReader::getDouble(data, "roll");
				sensorConfig.pitch = (float)JsonReader::getDouble(data, "pitch");
				sensorConfig.yaw = (float)JsonReader::getDouble(data, "yaw");

				mHostVehicleConfig.sensorConfigurations.dataSize++;
				logOutputInfo(mTaskId.c_str(), "Sensor Id: %d, Type: %s, hz: %d, Position: [%f, %f, %f], Rotation: [%f, %f, %f]",
					sensorConfig.sensorId, sensorConfig.sensorType, sensorConfig.hz, sensorConfig.x, sensorConfig.y, sensorConfig.z,
					sensorConfig.roll, sensorConfig.pitch, sensorConfig.yaw);
			}
		}
		memset(mHostVehicleConfig.wayPoints.mainVehicleId, 0, SENSOR_IDTYPE_MAX);
		memcpy(mHostVehicleConfig.wayPoints.mainVehicleId, strVehicleId.c_str(), strVehicleId.size());
		mHostVehicleConfig.wayPoints.wayPointsSize = 0;
		if (JsonReader::hasValue(workRoot[i], "wayPoints")) {
			logOutputInfo(mTaskId.c_str(), "Task Ros way Poinst Enabled!");
			auto jsonWayPoints = JsonReader::getChild(workRoot[i], "wayPoints");
			for (int i = 0; i < jsonWayPoints.size(); ++i)
			{		
				SimOne_Data_WayPoints_Entry& wayPointsConfig = mHostVehicleConfig.wayPoints.wayPoints[mHostVehicleConfig.wayPoints.wayPointsSize];
				auto& jsonWayPoint = jsonWayPoints[i];
				wayPointsConfig.posX = static_cast<float>(JsonReader::getDouble(jsonWayPoint, "x"));
				wayPointsConfig.posY = static_cast<float>(JsonReader::getDouble(jsonWayPoint, "y"));
				wayPointsConfig.index = i;
				mHostVehicleConfig.wayPoints.wayPointsSize++;
			}
		}

		// customize control module
		if (JsonReader::hasValue(workRoot[i], "customization")) {
			auto customizationJson = JsonReader::getChild(workRoot[i], "customization");
			if (JsonReader::hasValue(customizationJson, "path")) {
				std::string path = JsonReader::getString(customizationJson, "path");
				if (!path.empty()) {
					//mCustomModule = new CustomizationModule;
					mHostVehicleConfig.bIsCustomModule = true;
					mHostVehicleConfig.customModule.url = path;
					mHostVehicleConfig.customModule.suffix = JsonReader::getString(customizationJson, "suffix");
					mHostVehicleConfig.customModule.entry = JsonReader::getString(customizationJson, "entry");
					mHostVehicleConfig.customModule.platform = JsonReader::getString(customizationJson, "platform");
					mHostVehicleConfig.customModule.target = JsonReader::getString(customizationJson, "target");

				}
				if (JsonReader::hasValue(customizationJson, "langType")) {
					mHostVehicleConfig.customModule.langType = JsonReader::getString(customizationJson, "langType");
				}
				if (JsonReader::hasValue(customizationJson, "interpretedScript")) {
					mHostVehicleConfig.customModule.interpretedScript = JsonReader::getString(customizationJson, "interpretedScript");
					if (mHostVehicleConfig.customModule.langType == "compiled") {
					}
					else if (mHostVehicleConfig.customModule.langType == "interpreted") {
						mHostVehicleConfig.customModule.entry = mHostVehicleConfig.customModule.interpretedScript;
					}

				}

				logOutputInfo(mTaskId.c_str(), "config url=====>:%s,suffix:%s,entry:%s,platform:%s,target:%s,langType:%s",
					mHostVehicleConfig.customModule.url.c_str(),
					mHostVehicleConfig.customModule.suffix.c_str(),
					mHostVehicleConfig.customModule.entry.c_str(),
					mHostVehicleConfig.customModule.platform.c_str(),
					mHostVehicleConfig.customModule.target.c_str(),
					mHostVehicleConfig.customModule.langType.c_str());
			}
			std::string driverAidedStr = JsonReader::getString(customizationJson, "aideController");
			mHostVehicleConfig.isDriverAssited = !driverAidedStr.empty();
		}

		if (cybertron::JsonReader::hasValue(workRoot[i], "controllers")) {
			auto controllers = JsonReader::getChild(workRoot[i], "controllers");
			mHostVehicleConfig.simOneAPITotalCount = (int)controllers.size();
			mHostVehicleConfig.bWaitSimOneAPI = true;
			logInfo("main vehicle id:%s SimOneAPITotalCount:%d", mHostVehicleConfig.vehicleId.c_str(), mHostVehicleConfig.simOneAPITotalCount);
		}
		mHostVehicleConfigDataMap[mHostVehicleConfig.vehicleId]=mHostVehicleConfig;
	}
 	mbGetTask = true;
 	return true;
}

void BridgeConfig::zeroMembers()
{
	mbGetTask = false;
	mHostVehicleConfigDataMap.clear();
}

std::string BridgeConfig::getStartScriptPath(std::string hostVehicle) {
	for (auto item = mHostVehicleConfigDataMap.begin(); item != mHostVehicleConfigDataMap.end(); item++) {
		if (item->first == hostVehicle) {
			return item->second.startScriptPath;
		}
	}
	return nullptr;
}
std::string BridgeConfig::getEndScriptPath(std::string hostVehicle) {
	for (auto item = mHostVehicleConfigDataMap.begin(); item != mHostVehicleConfigDataMap.end(); item++) {
		if (item->first == hostVehicle) {
			return item->second.endScriptPath;
		}
	}
	return nullptr;
}
std::vector<BridgeConfig::NodeInstance>* BridgeConfig::nodes(std::string hostVehicle) {
	for (auto item = mHostVehicleConfigDataMap.begin(); item != mHostVehicleConfigDataMap.end(); item++) {
		if (item->first == hostVehicle) {
			return &item->second.nodes;
		}
	}
	return nullptr;
}
const SensorGpsImuConfig&  BridgeConfig::sensorGpsImuConfig(std::string hostVehicle) {
	for (auto item = mHostVehicleConfigDataMap.begin(); item != mHostVehicleConfigDataMap.end(); item++) {
		if (item->first == hostVehicle) {
			return item->second.sensorGPSImuConfig;
		}
	}
	static SensorGpsImuConfig defaultSensorGpsImuConfig;
	return defaultSensorGpsImuConfig;
}
SimOne_Data_SensorConfigurations* BridgeConfig::SensorConfigurations(std::string hostVehicle) {
	for (auto item = mHostVehicleConfigDataMap.begin(); item != mHostVehicleConfigDataMap.end(); item++) {
		if (item->first == hostVehicle) {
			return &item->second.sensorConfigurations;
		}
	}
	return nullptr;
}
SimOne_Data_WayPoints* BridgeConfig::wayPointsConfig(std::string hostVehicle) {
	for (auto item = mHostVehicleConfigDataMap.begin(); item != mHostVehicleConfigDataMap.end(); item++) {
		if (item->first == hostVehicle) {
			return &item->second.wayPoints;
		}
	}
	return nullptr;
}
bool BridgeConfig::isDriverSet(std::string hostVehicle) {
	for (auto item = mHostVehicleConfigDataMap.begin(); item != mHostVehicleConfigDataMap.end(); item++) {
		if (item->first == hostVehicle) {
			return item->second.driverSet;
		}
	}
	return false;
}
bool BridgeConfig::isDriverAssited(std::string hostVehicle) {
	for (auto item = mHostVehicleConfigDataMap.begin(); item != mHostVehicleConfigDataMap.end(); item++) {
		if (item->first == hostVehicle) {
			return item->second.isDriverAssited;
		}
	}
	return false;
}
BridgeConfig::CustomizationModule* BridgeConfig::customizationModuleConf(std::string hostVehicle) {
	for (auto item = mHostVehicleConfigDataMap.begin(); item != mHostVehicleConfigDataMap.end(); item++) {
		if (item->first == hostVehicle) {
			return &(item->second.customModule);
		}
	}
	return nullptr;
}
void BridgeConfig::AddSimOneAPICount(std::string hostVehicle) {
	for (auto item = mHostVehicleConfigDataMap.begin(); item != mHostVehicleConfigDataMap.end(); item++) {
		if (item->first == hostVehicle) {
			item->second.simOneAPICurCount++;
			if (item->second.simOneAPICurCount == item->second.simOneAPITotalCount) {
				item->second.bWaitSimOneAPI = false;
			}
		}
	}
}
bool BridgeConfig::IsWaitSimOneAPI(std::string hostVehicle) {
	for (auto item = mHostVehicleConfigDataMap.begin(); item != mHostVehicleConfigDataMap.end(); item++) {
		if (item->first == hostVehicle) {
			return item->second.bWaitSimOneAPI;
		}
	}
	return false;
}
int BridgeConfig::GetSimOneAPITotalCount(std::string hostVehicle) {
	for (auto item = mHostVehicleConfigDataMap.begin(); item != mHostVehicleConfigDataMap.end(); item++) {
		if (item->first == hostVehicle) {
			return item->second.simOneAPITotalCount;
		}
	}
	return 0;
}
BridgeConfig &BridgeConfig::getInstance()
{
	static BridgeConfig me;
	return me;
}

CYBERTRON_END